/**
  ******************************************************************************
  * @file    Project/STM32F10x_StdPeriph_Template/stm32f10x_it.c 
  * @author  MCD Application Team
  * @version V3.5.0
  * @date    08-April-2011
  * @brief   Main Interrupt Service Routines.
  *          This file provides template for all exceptions handler and 
  *          peripherals interrupt service routine.
  ******************************************************************************
  * @attention
  *
  * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
  * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
  * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
  * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
  * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
  * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
  *
  * <h2><center>&copy; COPYRIGHT 2011 STMicroelectronics</center></h2>
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_it.h"

/** @addtogroup STM32F10x_StdPeriph_Template
  * @{
  */

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/

float Kp_direccion = -16.57;
float Ki_direccion = 0;
float Kd_direccion = -29.37;
float Error_Acumulado_direccion = 0;
float Error_Anterior_direccion = 0;
float Resultado_PID_Direccion = 0;

float Kp_gas = 130;	//118 
float Ki_gas = 0;
float Kd_gas = 22;;
float Error_Acumulado_gas = 0;
float Error_Anterior_gas = 0;
float Resultado_PID_gas = 0;

float consigna_velocidad_m_s = CONSIGNA_VELOCIDAD_ZONA_INICIAL;

uint32_t ms_ignora_fc_puertas = 0;
uint32_t espera_inicial_ms = ESPERA_INICIAL_MS;
uint32_t estado_calibracion = ESPERA_INICIAL;

uint32_t Medida_Velocidad_Timer = 0;
float Array_filtro_pb_velocidad[NUM_MEDIDAS_FILTRO_PB_VELOCIDAD];
uint32_t index_filtro_pb_velocidad = 0;
int64_t Pulsos_Encoder_Total_64_anterior = 0;
float Metros_Desde_Ultima_Puerta = 0;
uint32_t Beep_Permanente=0;
int64_t Pulsos_Encoder_Total_64 = 0;
float Distancia_Total_metros = 0;
float Velocidad_metros_s = 0;
int32_t m_Bias_Bifurcacion = 0;

/* External variables --------------------------------------------------------*/
extern int32_t Bias_Bifurcacion;
extern uint32_t ms_restantes_beep;
extern uint32_t numero_puertas_atravesadas;
extern uint32_t Luminosidad_Barra_Frontal[];
extern uint32_t Luminosidad_Barra_Trasera[];
extern float Calibraciones_Frontal_Oscuro[];
extern float Calibraciones_Trasera_Oscuro[];
extern float Calibraciones_Frontal_Claro[];
extern float Calibraciones_Trasera_Claro[];
extern float rango_dinamico_frontal[24];
extern float rango_dinamico_trasera[16];
extern float consigna_posicion;
extern uint32_t Calibraciones_Frontal_OK[];
extern uint32_t Calibraciones_Trasera_OK[];
extern uint32_t EntradaModoControl;
extern uint32_t EntradaDireccion;
extern uint32_t EntradaAceleracion;
extern uint16_t EntradaAux;
extern uint32_t calibrando_barras;
extern float Posicion_Linea;
extern uint32_t vueltas_main_debug;



/******************************************************************************/
/*            Cortex-M3 Processor Exceptions Handlers                         */
/******************************************************************************/

/**
  * @brief  This function handles NMI exception.
  * @param  None
  * @retval None
  */
void NMI_Handler(void)
{
}

/**
  * @brief  This function handles Hard Fault exception.
  * @param  None
  * @retval None
  */
void HardFault_Handler(void)
{
  /* Go to infinite loop when Hard Fault exception occurs */
  while (1)
  {
	int kkkk;
	kkkk++;
  }
}

/**
  * @brief  This function handles Memory Manage exception.
  * @param  None
  * @retval None
  */
void MemManage_Handler(void)
{
  /* Go to infinite loop when Memory Manage exception occurs */
  while (1)
  {
  }
}

/**
  * @brief  This function handles Bus Fault exception.
  * @param  None
  * @retval None
  */
void BusFault_Handler(void)
{
  /* Go to infinite loop when Bus Fault exception occurs */
  while (1)
  {
  }
}

/**
  * @brief  This function handles Usage Fault exception.
  * @param  None
  * @retval None
  */
void UsageFault_Handler(void)
{
  /* Go to infinite loop when Usage Fault exception occurs */
  while (1)
  {
  }
}

/**
  * @brief  This function handles SVCall exception.
  * @param  None
  * @retval None
  */
void SVC_Handler(void)
{
}

/**
  * @brief  This function handles Debug Monitor exception.
  * @param  None
  * @retval None
  */
void DebugMon_Handler(void)
{
}

/**
  * @brief  This function handles PendSVC exception.
  * @param  None
  * @retval None
  */
void PendSV_Handler(void)
{
}

/**
  * @brief  This function handles SysTick Handler.
  * @param  None
  * @retval None
  */
void SysTick_Handler(void)
{
}

/******************************************************************************/
/*                 STM32F10x Peripherals Interrupt Handlers                   */
/*  Add here the Interrupt Handler for the used peripheral(s) (PPP), for the  */
/*  available peripheral interrupt handler's name please refer to the startup */
/*  file (startup_stm32f10x_xx.s).                                            */
/******************************************************************************/

// Variables relacionadas con la lectura de los sensores IR
uint32_t Estado_Lectura_IRS = IR_INACTIVO;
uint16_t IRS_Por_Leer_PortB = 0x0000;
uint16_t IRS_Por_Leer_PortC = 0x0000;
uint16_t IRS_Por_Leer_PortD = 0x0000;
uint16_t IRS_Por_Leer_PortE = 0x0000;
uint32_t contador_periodo_barras_ms = 0;
uint32_t FLAG_Comienza_Lectura_Barras=1;
uint32_t FLAG_lectura_ir_finalizada=1;

uint32_t PWM_IN_ciclo = 0;
uint32_t contador_perdida_senal = 0;
uint32_t memoria_TIM4CCR1 = 0;
uint32_t estado_PWM_IN=PWM_IN_Leer_Switch;
uint32_t USART1_Byte_Timer=~0;
uint32_t milisegundos_medicion_ciclo_scan = 0;
uint32_t cuenta_calibracion=0;

//Interrupcion del Timer1, a una frecuencia de 1Khz.
float ciclo_scan_ms;
void TIM1_UP_IRQHandler(void)
{
	if (!TIM_GetITStatus(TIM1, TIM_IT_Update)) 
		return;

	TIM_ClearITPendingBit(TIM1, TIM_IT_Update);

//	if (milisegundos_medicion_ciclo_scan++ > 1000)
//	{
//		ciclo_scan_ms = (float)milisegundos_medicion_ciclo_scan / vueltas_main_debug;
//		milisegundos_medicion_ciclo_scan=0;
//		vueltas_main_debug=0;
//	}

	if ((contador_periodo_barras_ms++ > IR_TIMEOUT) && FLAG_lectura_ir_finalizada)
	{
		FLAG_lectura_ir_finalizada = 0; 
		FLAG_Comienza_Lectura_Barras = 1;
		contador_periodo_barras_ms = 0;
	}

//	//Acumulacion del encoder y calculo de velocidad.
//	if (!GPIO_ReadInputDataBit(Puerto_Boton,Pin_Boton))	
    //	{
//		Pulsos_Encoder_Total_64 = 0;
//	}

	if (Medida_Velocidad_Timer++ >= PERIODO_MEDIDA_VELOCIDAD_MS)
	{
		int32_t latch_cuenta = TIM3->CNT;
		TIM3->CNT = 0;
		Medida_Velocidad_Timer = 0;
		
		if (latch_cuenta > 60000) 
		{
			latch_cuenta = latch_cuenta - 0xFFFF - 1;  // Numero negativo
		}
		Pulsos_Encoder_Total_64 += latch_cuenta;

		Velocidad_metros_s = (Pulsos_Encoder_Total_64 - Pulsos_Encoder_Total_64_anterior)*FACTOR_CONVERSION_ENCODER;

		Pulsos_Encoder_Total_64_anterior = Pulsos_Encoder_Total_64;
		Distancia_Total_metros = Pulsos_Encoder_Total_64 * METROS_POR_PULSO;
		Metros_Desde_Ultima_Puerta += latch_cuenta * METROS_POR_PULSO;
		
		Navegacion (); //Actualiza velocidad y direccion en funcion de nuestra localizacion en el mapa
		
		if (Velocidad_metros_s < consigna_velocidad_m_s * 0.5)
			Resultado_PID_gas = 60;
		else
			Resultado_PID_gas = Control_PID(Kp_gas, Ki_gas, Kd_gas,Velocidad_metros_s,consigna_velocidad_m_s,&Error_Acumulado_gas,&Error_Anterior_gas);
		
	}

	if ((USART1_Byte_Timer!=0) && (USART1_Byte_Timer!=~0)) USART1_Byte_Timer--;

	if(ms_restantes_beep>0) ms_restantes_beep--;
	if(ms_ignora_fc_puertas>0) ms_ignora_fc_puertas--;
	
	if (Beep_Permanente || (ms_restantes_beep>0))
		GPIO_WriteBit(GPIOA,GPIO_Pin_3,!GPIO_ReadOutputDataBit(GPIOA,GPIO_Pin_3));
	else 
		GPIO_WriteBit(GPIOA,GPIO_Pin_3,Bit_RESET);

	

	// Control de la variable 'PWM_IN_ciclo', que cuenta los milisegundos restantes hasta el siguiente cambio de pata de entrada PWM
	if (PWM_IN_ciclo > 0)
	{
		PWM_IN_ciclo--;
	}
	else // Si PWM_IN_ciclo vale 0, procedemos a guardar la entrada PWM,cambiar la pata de entrada y reiniciar la variable PWM_IN_ciclo;
	{
		uint32_t aux;
		TIM_ICInitTypeDef TIM_ICInitStructure;
		TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
		TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
		TIM_ICInitStructure.TIM_ICPrescaler = 0;
		TIM_ICInitStructure.TIM_ICFilter = 0x0;

		PWM_IN_ciclo = PWM_IN_TIEMPO_CICLO;

		//Rotacion PWM:
		switch(estado_PWM_IN)
		{
			case PWM_IN_Leer_Switch:
				aux = TIM4->CCR1;

				if (memoria_TIM4CCR1 == aux)
				{	
					if (contador_perdida_senal<TIMEOUT_SENAL) contador_perdida_senal++;
				}
				else contador_perdida_senal=0;

				if(contador_perdida_senal >= TIMEOUT_SENAL) 
				{
					EntradaModoControl = PARADA_EMERGENCIA;
					ms_restantes_beep = 500;
				}
			  else EntradaModoControl = (aux > CENTRO_AUX) ? CONTROL_MANUAL : CONTROL_AUTOMATICO;

				memoria_TIM4CCR1 = aux;
				
				estado_PWM_IN = PWM_IN_Leer_Aceleracion;
				GPIO_PinRemapConfig(GPIO_Remap_TIM4,DISABLE);
			  TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
			  TIM_PWMIConfig(TIM4, &TIM_ICInitStructure);
			  TIM_SelectInputTrigger(TIM4, TIM_TS_TI1FP1);
				break;

			case PWM_IN_Leer_Aceleracion:
				EntradaAceleracion = TIM4->CCR2;
				estado_PWM_IN = PWM_IN_Leer_Direccion;
				GPIO_PinRemapConfig(GPIO_Remap_TIM4,ENABLE);
			  TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
			  TIM_PWMIConfig(TIM4, &TIM_ICInitStructure);
			  TIM_SelectInputTrigger(TIM4, TIM_TS_TI1FP1);
				break;

			case PWM_IN_Leer_Direccion:
				EntradaDireccion = TIM4->CCR2;
				estado_PWM_IN = PWM_IN_Leer_Aux;
				GPIO_PinRemapConfig(GPIO_Remap_TIM4,DISABLE);
			  TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
			  TIM_PWMIConfig(TIM4, &TIM_ICInitStructure);
			  TIM_SelectInputTrigger(TIM4, TIM_TS_TI2FP2);
				break;
			
			case PWM_IN_Leer_Aux:
				EntradaAux = TIM4->CCR1;
				estado_PWM_IN = PWM_IN_Leer_Switch;
				GPIO_PinRemapConfig(GPIO_Remap_TIM4,ENABLE);
			  TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
			  TIM_PWMIConfig(TIM4, &TIM_ICInitStructure);
			  TIM_SelectInputTrigger(TIM4, TIM_TS_TI2FP2);
				break;
		}
	}


		//Control de las salidas
		if (espera_inicial_ms == 0)
		{
			if (!calibrando_barras) 
				Control_Vehiculo();
		}
		else 
		{
			espera_inicial_ms--;
		}

}

//-------------------------USARTx global interrupt handler----------------------------------------
	 	
	//Variables de recepcion
  uint8_t USART1_estado_rx = USART1_RX_ESTADO_CABECERA;
  uint8_t USART1_pila_rx[260];
	uint32_t USART1_pila_crc_rx[260];
  uint16_t USART1_pila_rx_indice=0;
	uint16_t USART1_bytes_restantes=0;
	uint8_t stuffing=0;
	uint8_t USART1_crc_ok_rx=0;
	
	//Variables de envio
	uint8_t USART1_estado_TX = USART1_TX_ESTADO_CABECERA;
	uint32_t USART1_pila_TX[260];
	uint8_t USART1_stuffing_TX=0;
	uint8_t USART1_TX_bytes_restantes = 0;
	uint16_t USART1_pila_TX_indice=0;
	uint8_t CRC_TX;

void USART1_IRQHandler(void)
{
  
/*	
	int a;
	a=20;	
	if(USART_GetITStatus(USART1, USART_IT_PE) != RESET)		
		a=0;
	if(USART_GetITStatus(USART1, USART_IT_TC) != RESET)		
		a=2;
	if(USART_GetITStatus(USART1, USART_IT_IDLE) != RESET)		
		a=4;
	if(USART_GetITStatus(USART1, USART_IT_LBD) != RESET)		
		a=5;
	if(USART_GetITStatus(USART1, USART_IT_CTS) != RESET)		
		a=6;
	if(USART_GetITStatus(USART1, USART_IT_ERR) != RESET)		
		a=7;
	if(USART_GetITStatus(USART1, USART_IT_ORE) != RESET)		
		a=8;
	if(USART_GetITStatus(USART1, USART_IT_NE) != RESET)		
		a=9;
	if(USART_GetITStatus(USART1, USART_IT_FE) != RESET)		
		a=10;

	if ((a != 20))
		a++;
*/

	//INTERRUPCION DE RECEPCION
  if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
  {
  	uint8_t rx_byte;
    /* Read one byte from the receive data register */
    //RxBuffer[RxCounter++] = (USART_ReceiveData(USART1) & 0x7F);
		rx_byte = (USART_ReceiveData(USART1) & 0xFF);


		//Byte stuffing para evitar ciclos infinitos en los que se confunde un byte normal por la cabecera
		if (!stuffing)
		{
			if((USART1_estado_rx != USART1_RX_ESTADO_CABECERA)&&(rx_byte == USART1_CABECERA))
			{
				stuffing=1;
				USART1_Byte_Timer = USART1_RX_TIMEOUT_MS;
			}
			else
			{
				USART1_pila_rx[USART1_pila_rx_indice]=rx_byte; // Introducimos en la pila e incrementamos el indice ("push")
				USART1_pila_crc_rx[USART1_pila_rx_indice++]=0xFF&rx_byte; // Introducimos en la pila e incrementamos el indice ("push")
			}
		}
		else
		{
			stuffing=0;
			if (rx_byte == USART1_CABECERA)
			{
				USART1_pila_rx[USART1_pila_rx_indice]=rx_byte; // Introducimos en la pila e incrementamos el indice ("push")
				USART1_pila_crc_rx[USART1_pila_rx_indice++]=0xFF&rx_byte; // Introducimos en la pila e incrementamos el indice ("push")
			}
			
			else
			{
				//Esperabamos otro caracter 'cabecera' por el stuffing pero ha llegado otra cosa.
				USART1_Reiniciar_Maquina_rx();
				//USART1_EnviarNACK();
			}
		}

	 	if (!stuffing)
		{
			switch(USART1_estado_rx)
			{
				//Recibiendo cabecera
				case USART1_RX_ESTADO_CABECERA:
					if ( rx_byte == USART1_CABECERA )
					{
						USART1_estado_rx = USART1_RX_ESTADO_COMANDO;
						USART1_Byte_Timer = USART1_RX_TIMEOUT_MS;
					}

					
					USART1_pila_rx_indice = 0;
					break;
		
				//Recibiendo el numero del tipo de comando
				case USART1_RX_ESTADO_COMANDO:
					if (USART1_ES_COMANDO_LARGO(rx_byte))
					{
							USART1_estado_rx = USART1_RX_ESTADO_NBYTES;
							USART1_Byte_Timer = USART1_RX_TIMEOUT_MS;
					}
					else if (USART1_ES_COMANDO_CORTO(rx_byte))
					{
							USART1_estado_rx = USART1_RX_ESTADO_CRC;
							USART1_Byte_Timer = USART1_RX_TIMEOUT_MS;
					}
					else 
					{
						USART1_Reiniciar_Maquina_rx();
						//USART1_EnviarNACK();
					}
					break;
		
				//Recibiendo el numero de bytes de un comando largo
				case USART1_RX_ESTADO_NBYTES:
					USART1_estado_rx=USART1_RX_ESTADO_NOT_NBYTES;
					USART1_Byte_Timer = USART1_RX_TIMEOUT_MS;				
					break;
	
				//Recibiendo el numero de bytes de un comando largo, invertido (redundancia)
				case USART1_RX_ESTADO_NOT_NBYTES:
					if(((~rx_byte)&0xFF) == (USART1_pila_rx[USART1_pila_rx_indice-2]&0xFF))
					{				
						USART1_estado_rx=USART1_RX_ESTADO_BUCLE;
						USART1_bytes_restantes = USART1_pila_rx[USART1_pila_rx_indice-2];
						USART1_Byte_Timer = USART1_RX_TIMEOUT_MS;
					}
					else
					{
						USART1_Reiniciar_Maquina_rx();
						//USART1_EnviarNACK();
					}
					break;
	
				//Bucle: Recibiendo un byte de comando largo (MEJORAR EXPLICACION)
				case USART1_RX_ESTADO_BUCLE:
					USART1_Byte_Timer = USART1_RX_TIMEOUT_MS;
					if(--USART1_bytes_restantes == 0)
						USART1_estado_rx	= USART1_RX_ESTADO_CRC;
					break;
	
				//Recibiendo CRC: Es el tercer byte en comandos cortos, o el byte 4+n+1 en comandos largos. Es el ultimo byte de la transmision.
				case USART1_RX_ESTADO_CRC:

					CRC_ResetDR();	//Reset del registro de datos del modulo CRC
					if (USART1_ES_COMANDO_CORTO(USART1_pila_rx[0]))	//Si es comando corto, solo hay un byte al que hacer CRC
					{
						USART1_crc_ok_rx = ((CRC_CalcCRC(USART1_pila_rx[0]) & 0xFF) == USART1_pila_rx[1]);
					}
					else if (USART1_ES_COMANDO_LARGO(USART1_pila_rx[0])) //Si es comando largo, hay que hacer CRC a todo el array (A partir del comando y hasta el ultimo 
					{
						USART1_crc_ok_rx = (CRC_CalcBlockCRC(USART1_pila_crc_rx, USART1_pila_rx[1]+3) & 0xFF ) == USART1_pila_rx[USART1_pila_rx[1] + 3];
					}
					else
						USART1_crc_ok_rx = 0;
					
					if (USART1_crc_ok_rx) 
					{		
						USART_ITConfig(USART1, USART_IT_RXNE, DISABLE); //Deshabilitamos interrupciones de recepcion de USART1 hasta que terminemos de procesar lo recibido
						USART1_procesar_rx();
					}
					else
					{
						while(0==1-1);	//Para ver si falla el CRC (solo para debug)
					}
					USART1_Reiniciar_Maquina_rx();

					break;
	
				default: // no deberia llegar aqui nunca
					USART1_Reiniciar_Maquina_rx();
					break;
			}
		}
  }
	//
	//
  //INTERRUPCION DE TRANSMISION
	if(USART_GetITStatus(USART1, USART_IT_TXE) != RESET) 
	{
		uint8_t tx_byte;
		
		if (USART1_ES_COMANDO_LARGO(USART1_pila_TX[0]) || USART1_ES_COMANDO_CORTO(USART1_pila_TX[0])) {
		  
			switch (USART1_estado_TX) {
				//Enviando cabecera
				case USART1_TX_ESTADO_CABECERA:
					USART_SendData(USART1, USART1_CABECERA);
					USART1_estado_TX = USART1_TX_ESTADO_COMANDO;
					break;
				
				//Enviando comando
				case USART1_TX_ESTADO_COMANDO:
					tx_byte = USART1_pila_TX[USART1_pila_TX_indice++];
					USART_SendData(USART1, tx_byte);
					
					if (USART1_ES_COMANDO_LARGO(tx_byte)) 
						USART1_estado_TX = USART1_TX_ESTADO_NBYTES;
					else if (USART1_ES_COMANDO_CORTO(tx_byte))
						USART1_estado_TX = USART1_TX_ESTADO_CRC;
					break;
				
				//Enviamos el numero de bytes	
				case USART1_TX_ESTADO_NBYTES:
					tx_byte = USART1_pila_TX[USART1_pila_TX_indice++];
					USART_SendData(USART1, tx_byte);
					
					if ((USART1_stuffing_TX == 0) && (((0xFF) & tx_byte) == ((0xFF) & USART1_CABECERA))) {
						USART1_stuffing_TX = 1;
						USART1_pila_TX_indice--;
					}else if (USART1_stuffing_TX == 0) {
						USART1_estado_TX = USART1_TX_ESTADO_NOT_NBYTES;
						USART1_TX_bytes_restantes = tx_byte;
					}else {
						USART1_stuffing_TX = 0;
						USART1_estado_TX = USART1_TX_ESTADO_NOT_NBYTES;
						USART1_TX_bytes_restantes = tx_byte;
					}
					
					break;
					
				//Enviamos el numbero de bites negado
				case USART1_TX_ESTADO_NOT_NBYTES:
					tx_byte = USART1_pila_TX[USART1_pila_TX_indice++];
					USART_SendData(USART1, tx_byte);
					
					if ((USART1_stuffing_TX == 0) && (((0xFF) & tx_byte) == ((0xFF) & USART1_CABECERA))) {
						USART1_stuffing_TX = 1;
						USART1_pila_TX_indice--;
					}else if (USART1_stuffing_TX == 1){
						USART1_stuffing_TX = 0;
					}
					
					//Si no estamos rellenando o el relleno ha terminado
					if (USART1_stuffing_TX == 0) {
						USART1_estado_TX = USART1_TX_ESTADO_BUCLE;
					}
					
					break;
					
				//Enviamos los datos extra que incluye la trama
				case USART1_TX_ESTADO_BUCLE:
					tx_byte = USART1_pila_TX[USART1_pila_TX_indice++];
					USART_SendData(USART1, tx_byte);
					
					if ((USART1_stuffing_TX == 0) && (((0xFF) & tx_byte) == ((0xFF) & USART1_CABECERA))) {
						USART1_stuffing_TX = 1;
						USART1_pila_TX_indice--;
					}else if (USART1_stuffing_TX == 1){
						USART1_stuffing_TX = 0;
					}
					
					//Si no estamos rellenando y el rellenado ha terminado y no quedan mas bytes
					if ((USART1_stuffing_TX == 0) && ((--USART1_TX_bytes_restantes) == 0)) {
						USART1_estado_TX = USART1_TX_ESTADO_CRC;
					}
					
					break;


				//Calculamos el CRC (habria que tener en cuenta el rellenado tambien)								                              
				case USART1_TX_ESTADO_CRC:

					if (!stuffing)
					{
						if (USART1_ES_COMANDO_CORTO(USART1_pila_TX[0]))	//Si es comando corto, solo hay un byte al que hacer CRC
						{
						  CRC_ResetDR();	//Reset del registro de datos del modulo CRC
							CRC_TX = (uint8_t)(CRC_CalcCRC(USART1_pila_TX[0]) & 0xFF);
						}
						else if (USART1_ES_COMANDO_LARGO(USART1_pila_TX[0])) //Si es comando largo, hay que hacer CRC a todo el array (A partir del comando y hasta el ultimo 
						{
							CRC_ResetDR();	//Reset del registro de datos del modulo CRC
							CRC_TX = (uint8_t)(CRC_CalcBlockCRC(USART1_pila_TX, USART1_pila_TX[1]+3) & 0xFF);
						}
						USART_SendData(USART1, CRC_TX);
					}

					if ((!USART1_stuffing_TX) && (((0xFF) & CRC_TX) == ((0xFF) & USART1_CABECERA))) {
						USART1_stuffing_TX = 1;
						USART1_pila_TX_indice--;		//irrelevante para el estado CRC???
					}else if (USART1_stuffing_TX){
						USART_SendData(USART1, USART1_CABECERA);
						USART1_stuffing_TX = 0;
					}
					
					//Si no estamos rellenando o el relleno ha terminado
					if (USART1_stuffing_TX == 0) {
						if ( USART1_pila_TX[0] == USART1_COMANDO_ACK)
						{
//						 	USssART1_procesar_rx(); // Procesamos la respues
						}

						USART1_estado_TX = USART1_TX_ESTADO_CABECERA;
						USART_ITConfig(USART1, USART_IT_TXE, DISABLE);	 //Desactivamos la interrupcion de transmision para que no nos interrumpa mas (hemos terminado de enviar)
						USART1_pila_TX_indice = 0;
					}
				
					break;
					
				default:
					USART1_estado_TX = USART1_TX_ESTADO_CABECERA;
					USART_ITConfig(USART1, USART_IT_TXE, DISABLE);	 //Desactivamos la interrupcion de transmision
					break;
			}
		}
	}

	USART_ClearITPendingBit(USART1, USART_IT_TC | USART_IT_IDLE | USART_IT_LBD | USART_IT_CTS | USART_IT_ERR | USART_IT_ORE | USART_IT_NE |USART_IT_FE);
}

void USART1_procesar_rx(void)
{	
	uint16_t i;
	switch (USART1_pila_rx[0])
	{
		// Comandos de escritura (Solo respondemos con ACK)
		case USART1_COMANDO_STOP:
		  USART1_Enviar_ACK();
			break;				
		
		
		case USART1_COMANDO_CONTROL_MANUAL: 	
			USART1_Enviar_ACK();
			break;
		
		
		case USART1_COMANDO_MAPEO_DIRECC:		
			USART1_Enviar_ACK();
			break;
		
		
		case USART1_COMANDO_MAPEO_ACELER:		
			USART1_Enviar_ACK();
			break;
		
		// Comandos de lectura (requieren respuesta)
		case USART1_COMANDO_LOOPBACK:
			for (i=0;i<USART1_pila_rx[1];i++)
			{
				USART1_pila_TX[i] = USART1_pila_rx[i];
			}
			USART_ITConfig(USART1, USART_IT_TXE , ENABLE);			
			break;	

        case USART1_COMANDO_SOLICITA_VAR:
            USART1_Enviar_Variable(USART1_pila_rx[3]);
            break;


//		case USART1_COMANDO_LEE_BARRAS: //Copiar al vector USART1_pila_TX los ultimos datos que se leyeron de la barra frontal y enviar. 
//		break;

		default: //No se ha comprendido el comando recibido, respondemos con "NACK"
			USART1_Enviar_NACK();
	}

	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
}

void USART1_Enviar_ACK(void)
{
    USART1_pila_TX[0]=USART1_COMANDO_ACK;
    //Habilita la interrupcion de transmision de la UART1 para comenzar a enviar.
    USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
}

void USART1_Enviar_NACK(void)
{
    USART1_pila_TX[0]=USART1_COMANDO_NACK;
    //Habilita la interrupcion de transmision de la UART1 para comenzar a enviar.
    USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
}

uint32_t USART1_Recibir_Variable(void)
{
    uint32_t varid;
    varid = USART1_pila_RX[3];
    switch(varid)
    {    
        case varid_Kp_gas:
            Kp_gas = *((float*)(&USART1_pila_RX[4]));
            break;
        case varid_Ki_gas:
            Ki_gas = *((float*)(&USART1_pila_RX[4]));
            break;
        case varid_Kd_gas:
            Kd_gas = *((float*)(&USART1_pila_RX[4]));
            break;
        default:
            break;
    }
    return varid;
}   

void USART1_Enviar_Variable(uint32_t varid)
{
    int i;
    char *punt_char;
    USART1_pila_TX[0]=USART1_COMANDO_ENVIA_VAR;
    switch(varid)
    {    
        case varid_Kp_gas:
            USART1_pila_TX[1]=sizeof(Kp_gas)+1;
            punt_char = (char*)&Kp_gas;       
            break;
        case varid_Ki_gas:
            USART1_pila_TX[1]=sizeof(Ki_gas)+1;
            punt_char = (char*)&Ki_gas;       
            break;
        case varid_Kd_gas:
            USART1_pila_TX[1]=sizeof(Kd_gas)+1;
            punt_char = (char*)&Kd_gas;       
            break;
        default:
            break;
    }
    USART1_pila_TX[2]=0xFF & ~USART1_pila_TX[1];
    USART1_pila_TX[3]=0xFF & varid;
    for (i=4;i<sizeof(Kd_gas)+4;i++)
    {
        USART1_pila_TX[i]=*(punt_char++);
    }
    USART_ITConfig(USART1, USART_IT_TXE , ENABLE);			
}

void USART1_Reiniciar_Maquina_rx()
{
	USART1_Byte_Timer = ~0; //El temporizador de timeout no se mueve
	USART1_pila_rx_indice=0;		 
	USART1_estado_rx = USART1_RX_ESTADO_CABECERA;
}
	float rd_medio_frontal = 0;
	float rd_medio_trasera = 0;
void Calibra_Barras(void)
{
	int i;
	uint32_t frontal_ok[24];
	uint32_t trasera_ok[16];

	switch (estado_calibracion)
	{
		case ESPERA_INICIAL:
			if (cuenta_calibracion >= 250)
			{
				estado_calibracion = CALIBRANDO_OSCURO;
				cuenta_calibracion=0;
				Beep_Permanente=1;
			}
			break;
			
		case CALIBRANDO_OSCURO:
			for (i=0;i<24;i++)
			{
				Calibraciones_Frontal_Oscuro[i] = Calibraciones_Frontal_Oscuro[i] + (float)Luminosidad_Barra_Frontal[i]/IR_NUM_CALIBRACION;
			}
			for (i=0;i<16;i++)
			{
				Calibraciones_Trasera_Oscuro[i] = Calibraciones_Trasera_Oscuro[i] + (float)Luminosidad_Barra_Trasera[i]/IR_NUM_CALIBRACION;			
			}
			if (cuenta_calibracion >= IR_NUM_CALIBRACION)
			{
				estado_calibracion = ESPERA_SUELTA_BOTON;
				Beep_Permanente=0;	
			}
			break;
		
		case ESPERA_SUELTA_BOTON:
			if (GPIO_ReadInputDataBit(Puerto_FC_Colision, Pin_FC_Colision)==0)
			{
				estado_calibracion = CALIBRANDO_CLARO;
				cuenta_calibracion = 0;
				Beep_Permanente=1;
			}
			break;

		case CALIBRANDO_CLARO:
			for (i=0;i<24;i++)
			{
				Calibraciones_Frontal_Claro[i] = Calibraciones_Frontal_Claro[i] + (float)Luminosidad_Barra_Frontal[i]/IR_NUM_CALIBRACION;
			}
			for (i=0;i<16;i++)
			{
				Calibraciones_Trasera_Claro[i] = Calibraciones_Trasera_Claro[i] + (float)Luminosidad_Barra_Trasera[i]/IR_NUM_CALIBRACION;			
			}
			if (cuenta_calibracion >= IR_NUM_CALIBRACION)
			{
				for (i=0;i<24;i++)
				{
					rango_dinamico_frontal[i] = Calibraciones_Frontal_Oscuro[i] - Calibraciones_Frontal_Claro[i];
					rd_medio_frontal += rango_dinamico_frontal[i] / 24;
				}
				for (i=0;i<16;i++)
				{
					rango_dinamico_trasera[i] = Calibraciones_Trasera_Oscuro[i] - Calibraciones_Trasera_Claro[i];
					rd_medio_trasera += rango_dinamico_trasera[i] / 24;
				}
				for (i=0;i<24;i++)
				{
					if (rango_dinamico_frontal[i] < 0.1*rd_medio_frontal) frontal_ok[i] = 0;
					else frontal_ok[i] = 1;
				}
				for (i=0;i<16;i++)
				{
					if (rango_dinamico_trasera[i] < 0.1*rd_medio_trasera) trasera_ok[i] = 0;
					else trasera_ok[i] = 1;
				}
				Beep_Permanente=0;
				calibrando_barras=0;
			}
			break;
	}
	cuenta_calibracion++;
}


	float testdir=0;
	float testaux=0;
	float testacc=0;

void Control_Vehiculo(void) 
{
	if (EntradaModoControl == CONTROL_MANUAL){
		//Control manual. Sacar a las salidas pwm los valores recibidos a la entrada.
		TIM2->CCR2 = /*0xFFFF &*/ EntradaAceleracion;
		TIM2->CCR1 = /*0xFFFF &*/ EntradaDireccion;
		TIM2->CCR3 = /*0xFFFF &*/ EntradaDireccion;
		Error_Acumulado_gas = 0;
		Error_Acumulado_direccion = 0;
		Beep_Permanente = 0;
		numero_puertas_atravesadas = 0;
	}
	else if (EntradaModoControl == CONTROL_AUTOMATICO)//Control automatico	// En esta funcion se debe implementar el control PID.
	{	
		if (GPIO_ReadInputDataBit(Puerto_FC_Colision,Pin_FC_Colision))
		{
			Salida_Throttle(0);
			ms_restantes_beep = 2;
		}
		else
		{
//			if (m_Bias_Bifurcacion != Bias_Bifurcacion)
//			{
//			 	m_Bias_Bifurcacion = Bias_Bifurcacion;
//				ms_restantes_beep = 500;
//			}
			Salida_Direccion(Resultado_PID_Direccion);
			Salida_Throttle(Resultado_PID_gas);
			//Salida_Direccion(Mapeo_Entrada_Dir());

		} 
	}
	else //parada de emergencia por perdida de senal
	{
		TIM2->CCR1=CENTRO_DIR;
		TIM2->CCR2=CENTRO_ACC;
		TIM2->CCR3=CENTRO_DIR;
	}
}

float Control_PID(float Kp,float Ki,float Kd,float Entrada,float Consigna,float *Error_Acumulado,float *Error_Anterior)
{
	float Error = Consigna - Entrada;
	float Salida;
	
	*Error_Acumulado += Error;
	Salida = Kp*Error + Kd*(Error-*Error_Anterior) + Ki*(*Error_Acumulado);	
	*Error_Anterior = Error;

	return Salida;
}

void Salida_Direccion(float porcentaje)
{
	float aux;
	
	aux = CENTRO_DIR - (porcentaje)*(MAX_GIRO_TRASERO_PWM)/100;
	if (aux>CENTRO_DIR+MAX_GIRO_TRASERO_PWM) 
	{
		//Beep_Permanente = 1;
		aux = CENTRO_DIR+MAX_GIRO_TRASERO_PWM;
	}
	else if (aux<CENTRO_DIR-MAX_GIRO_TRASERO_PWM) 
	{
		//Beep_Permanente = 1;
		aux = CENTRO_DIR-MAX_GIRO_TRASERO_PWM;
	}
	//else Beep_Permanente = 0;
	TIM2->CCR3 = aux;
	
	aux = CENTRO_DIR + porcentaje*(MAX_GIRO_DELANTERO_PWM)/100;
	if (aux>CENTRO_DIR+MAX_GIRO_DELANTERO_PWM)
	{
		//Beep_Permanente = 1; 
		aux = CENTRO_DIR+MAX_GIRO_DELANTERO_PWM;
	}
	else if (aux<CENTRO_DIR-MAX_GIRO_DELANTERO_PWM) 
	{
		//Beep_Permanente = 1;
		aux = CENTRO_DIR-MAX_GIRO_DELANTERO_PWM;
	}
	//else Beep_Permanente = 0;
	TIM2->CCR1 = aux;
}

void Salida_Throttle(float porcentaje)
{
	float aux = porcentaje*10;
	aux = CENTRO_ACC+aux;
	if (aux>CENTRO_ACC+MAX_ACC_PWM) 
	{
		aux = CENTRO_ACC + MAX_ACC_PWM;
	}
	else if (aux<CENTRO_ACC-MAX_ACC_PWM) 
	{
		aux = CENTRO_ACC-MAX_ACC_PWM;
	}
	//if (aux < CENTRO_ACC) Beep_Permanente = 1;
	//	else Beep_Permanente = 0;
	TIM2->CCR2 = aux;
}

float Mapeo_Entrada_Dir(void)
{
	 float x = (float)EntradaDireccion - CENTRO_DIR;
	 return 100* x / (MAXIMO_DIR - CENTRO_DIR);
}

float Mapeo_Entrada_Acc(void)
{
	 float x = (float)EntradaAceleracion - CENTRO_ACC;
	 return 100* x / (MAXIMO_ACC - CENTRO_ACC);
}

float Mapeo_Entrada_Aux(void)
{
	 float x = (float)EntradaAux - CENTRO_AUX;
	 return 100 * x / (MAXIMO_AUX - CENTRO_AUX);
}

void Navegacion (void)
{
	switch (numero_puertas_atravesadas) //esta variable se actualiza por interrupcion cuando el final de carrera de puertas se activa
	{
		case 0:
			Bias_Bifurcacion = 0;
			consigna_velocidad_m_s = 0;
			Error_Acumulado_direccion=0;
			break;
		case 1: //Justo al arrancar
			Bias_Bifurcacion = 0;
			consigna_velocidad_m_s = CONSIGNA_VELOCIDAD_ZONA_INICIAL;
			break;

		case 2: //Antes de la primera rampa
			Bias_Bifurcacion = 0;
			if ((Metros_Desde_Ultima_Puerta < 3)) consigna_velocidad_m_s = CONSIGNA_VELOCIDAD_RAMPAS;
			else consigna_velocidad_m_s = CONSIGNA_VELOCIDAD_ZONA_INICIAL;
			break;
										
		case 9: //Antes de bajar segunda rampa (despues de la puerta '8')
			if (Metros_Desde_Ultima_Puerta < DIST_8_A - MARGEN_BIFURCA) consigna_velocidad_m_s = CONSIGNA_VELOCIDAD_RAMPAS;
			if ((Metros_Desde_Ultima_Puerta > DIST_8_A - MARGEN_BIFURCA)&&(Metros_Desde_Ultima_Puerta < DIST_8_A + MARGEN_BIFURCA))
				{
					consigna_velocidad_m_s = CONSIGNA_VELOCIDAD_ZONA_DECISIONES;
					Bias_Bifurcacion = -1;
					Error_Acumulado_gas = 0;
					//Ki_gas = 0.05;
					//Kp_gas = 80;
				}
			else 
				Bias_Bifurcacion = 0;
			break;

		case 10:	//Despues de pasar la puerta '9'
			consigna_velocidad_m_s = CONSIGNA_VELOCIDAD_ZONA_DECISIONES;
			if ((Metros_Desde_Ultima_Puerta > MARGEN_GUARDA_PUERTAS)&&(Metros_Desde_Ultima_Puerta < (float)DIST_9_A*0.9))
				Bias_Bifurcacion = 1;
			else if ((Metros_Desde_Ultima_Puerta > (float)DIST_9_A*0.9))
				Bias_Bifurcacion = -1;
			break;

		case 11:	//Despues de pasar la puerta '10'
			if ((Metros_Desde_Ultima_Puerta > MARGEN_GUARDA_PUERTAS)&&(Metros_Desde_Ultima_Puerta < (float)DIST_10_A*0.9))
				Bias_Bifurcacion = -1;
			else if ((Metros_Desde_Ultima_Puerta > (float)DIST_10_A*0.9)&&(Metros_Desde_Ultima_Puerta < (float)DIST_10_A + (float)DIST_10_B/2))
				Bias_Bifurcacion = 1;
			else
				Bias_Bifurcacion = 0;
			break;

		case 12:	//Despues de pasar la puerta '11'
			if ((Metros_Desde_Ultima_Puerta > MARGEN_GUARDA_PUERTAS)&&(Metros_Desde_Ultima_Puerta < (float)DIST_11_A*0.8))
				Bias_Bifurcacion = 1;
			else if ((Metros_Desde_Ultima_Puerta > (float)DIST_11_A*0.8)&&(Metros_Desde_Ultima_Puerta < (float)DIST_11_A + (float)DIST_11_B/2))
				Bias_Bifurcacion = -1;
			else
				Bias_Bifurcacion = 0;
			break;

		case 13:	//Despues de pasar la puerta '12'
			if ((Metros_Desde_Ultima_Puerta > MARGEN_GUARDA_PUERTAS)&&(Metros_Desde_Ultima_Puerta < DIST_12_A + DIST_12_B - MARGEN_GUARDA_PUERTAS))
				Bias_Bifurcacion = 1;
			else
				Bias_Bifurcacion = 0;
			break;

		case 14:	//Despues de pasar la puerta '13'
			if ((Metros_Desde_Ultima_Puerta > MARGEN_GUARDA_PUERTAS)&&(Metros_Desde_Ultima_Puerta < (float)DIST_13_A*0.8))
				Bias_Bifurcacion = -1;
			else if ((Metros_Desde_Ultima_Puerta > (float)DIST_13_A*0.8)&&(Metros_Desde_Ultima_Puerta < (float)DIST_13_A+(float)DIST_13_B/2))
				Bias_Bifurcacion = 1;
			else
				Bias_Bifurcacion = 0;
			break;

		default:
			Bias_Bifurcacion = 0;
			consigna_velocidad_m_s = CONSIGNA_VELOCIDAD_ZONA_INICIAL;
			break;
			
	}
}

void EXTI9_5_IRQHandler(void)
{
  if(EXTI_GetITStatus(EXTI_Line8) != RESET)
  {
		/* Clear the  EXTI line 8 pending bit */
    EXTI_ClearITPendingBit(EXTI_Line8);
		
		if ((ms_ignora_fc_puertas == 0)&&(GPIO_ReadInputDataBit(Puerto_FC_Puertas, Pin_FC_Puertas) == 0)) 
		{
			ms_restantes_beep = MS_BEEP_PUERTAS;
			ms_ignora_fc_puertas = MS_IGNORA_PUERTAS;
			Metros_Desde_Ultima_Puerta = 0;
			numero_puertas_atravesadas++;
		}
  }
}
